#include <rclcpp/rclcpp.hpp>
#include <image_transport/image_transport.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/msg/image.hpp>
#include <opencv2/opencv.hpp>
#include "../include/detector.hpp"
#include <string>
using namespace std;
using namespace std::placeholders;
#include <opencv2/core/mat.hpp>
#include <sensor_msgs/msg/image.hpp>
//在fork仓库内部pr
using namespace std::chrono_literals;

class DetectorNode : public rclcpp::Node
{
public:
    DetectorNode() : Node("rm_detector_node")
    {
        // 设置参数
        this->declare_parameter("input_topic", "/camera/image_raw");           // 视频发布话题
        this->declare_parameter("output_topic", "/armor_detection/image_raw"); // 识别结果发布话题
        this->declare_parameter("video_path", "");

        // 获取参数
        input_topic = this->get_parameter("input_topic").as_string();
        output_topic = this->get_parameter("output_topic").as_string();
        video_path = this->get_parameter("video_path").as_string();

        // 视频读取
        if (!video_path.empty())
        {
            cap.open(video_path);
            if (cap.isOpened())
            {
                RCLCPP_INFO(this->get_logger(), "已经打开视频 %s", video_path.c_str());
                // 定时发布视频帧
                video_timer = this->create_wall_timer(33ms, bind(&DetectorNode::publishVideoFrame, this));
            }
            else
            {
                RCLCPP_ERROR(this->get_logger(), "无法打开视频 %s", video_path.c_str());
            }
        }

        RCLCPP_INFO(this->get_logger(), "节点开始了，订阅 %s，发布 %s",
                    input_topic.c_str(), output_topic.c_str());

        // 通过定时器延迟初始化（避免构造函数中使用shared_from_this()）
        init_timer = this->create_wall_timer(
            10ms, // 极短延迟，确保节点被shared_ptr管理
            std::bind(&DetectorNode::initImageTransport, this));
    }

private:
    rclcpp::TimerBase::SharedPtr init_timer;  // 初始化定时器
    rclcpp::TimerBase::SharedPtr video_timer; // 视频发布定时器
    string input_topic;
    string output_topic;
    string video_path;
    image_transport::Subscriber subscriber; // 图像订阅者
    image_transport::Publisher publisher;   // 识别结果发布者
    JianChe detector;                       // 装甲板识别器
    VideoCapture cap;                       // 视频读取对象
    // image_transport::Publisher video_publisher;   //视频帧发布者
    // image_transport::Subscriber video_subscriber;

    // 初始化图像传输
    void initImageTransport()
    {
        image_transport::ImageTransport it(this->shared_from_this());
        publisher = it.advertise(output_topic, 1); // 发布识别结果

        subscriber = it.subscribe(input_topic, 1, bind(&DetectorNode::showImageCallback, this, _1));
        // video_subscriber = it.subscribe(input_topic, 1, bind(&DetectorNode::imageCallback, this, _1));
        init_timer.reset(); // 初始化后销毁定时器，避免重复调用
    }

    void publishVideoFrame()
    {
        Mat frame;
        if (cap.read(frame) && !frame.empty())
        {
            Mat result = detector.ShiBei(frame);
            imshow("Armor Detection Result", result);
            waitKey(1);

            // 发布识别结果
            auto msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", result).toImageMsg();
            msg->header.stamp = this->now();
            msg->header.frame_id = "camera";
            publisher.publish(msg);
        }
        else
        {
            // 视频循环播放
            cap.set(cv::CAP_PROP_POS_FRAMES, 0);
            RCLCPP_INFO(this->get_logger(), "视频循环播放");
        }
    }

    void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr &msg)
    {
        try
        {
            // 将ros图像消息转换为opencv格式
            cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);

            // 调用识别
            Mat result = detector.ShiBei(cv_ptr->image);

            // 创建输出消息
            auto out_msg = cv_bridge::CvImage(msg->header, "bgr8", result).toImageMsg();

            // 发布处理后的图像
            // auto out_msg = cv_bridge::CvImage(msg->header, "bgr8", result).toImageMsg();
            publisher.publish(out_msg);
        }
        catch (cv_bridge::Exception &e)
        {
            RCLCPP_ERROR(this->get_logger(), "转换异常：%s", e.what());
        }
    }

    // 订阅识别图像并显示
    void showImageCallback(const sensor_msgs::msg::Image::ConstSharedPtr &msg)
    {
        try
        {
            // 转换ROS图像消息为OpenCV格式
            cv::Mat img = cv_bridge::toCvShare(msg, "bgr8")->image;
            // 显示图像窗口
            cv::namedWindow("Armor Detection Result", cv::WINDOW_NORMAL);
            cv::imshow("Armor Detection Result", img);
            cv::waitKey(1); // 刷新窗口
        }
        catch (cv_bridge::Exception &e)
        {
            RCLCPP_ERROR(this->get_logger(), "显示异常：%s", e.what());
        }
    }
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = make_shared<DetectorNode>();
    rclcpp::spin(node);
    destroyAllWindows();
    rclcpp::shutdown();
    return 0;
}
